#include "taskagent.h"
#include "../servo/servoagent.h"
#include "systemmonitor.h"
#include "../slice/cliread.h"
#include "../rtc5/rtc5agent.h"
#include <QDebug>

//#define Eprint

namespace HIM {

void TaskAgentThread::run()
{
    QTimer timer;
    connect(&timer,&QTimer::timeout,this,&TaskAgentThread::on_timeout,Qt::DirectConnection);
    timer.start(500);
    exec();
    return;
}

void TaskAgentThread::on_timeout()
{
    TASKAGENT.doTask();
}

typedef enum{
    NULLTASK,
    MoveFeedServoAvoid,
    MoveManServoAvoid,
    MoveFeedSpreading,
    MoveManSpreading,
    ScraperToZero,
    ScraperCover,
    CheckFeedInPlace,
    CheckManInPlace,
    CheckPowerInPlace,
    SlicelayerScan,
    GenerateCurrentLayer,
    PrintFinish,
} TaskTypeIndex;

moodycamel::ReaderWriterQueue<TaskTypeIndex> printTaskList;

double feedTargetPos{0};
double manTargetPos{0};
double powerTargetPos{0};

double avoidanceDistance{1.0};

TaskAgent::TaskAgent():
    taskAgentThread(new TaskAgentThread)
{

}

TaskAgent::~TaskAgent()
{
    taskAgentThread->quit();
    taskAgentThread->wait();
    delete taskAgentThread;
}

void TaskAgent::doTask()
{
    if(threadPause)
        return;

    qDebug()<<"doTask A:" << printTaskList.size_approx();


    if(printTaskList.peek()){

        qDebug()<<"doTask B:" << printTaskList.size_approx();

        TaskTypeIndex task = *printTaskList.peek();
        switch (task) {
        case NULLTASK : {
            printTaskList.pop();
        } break;
        case MoveFeedServoAvoid : {
            moveFeedServoAvoid();
            printTaskList.pop();
        } break;
        case MoveManServoAvoid : {
            moveManServoAvoid();
            printTaskList.pop();
        } break;
        case MoveFeedSpreading : {
            moveFeedSpreading();
            printTaskList.pop();
        } break;
        case MoveManSpreading : {
            moveManSpreading();
            printTaskList.pop();
        } break;
        case ScraperToZero : {
            scraperToZero();
            printTaskList.pop();
        } break;
        case ScraperCover: {
            scraperCover();
            printTaskList.pop();
        } break;
        case CheckFeedInPlace: {
            if(checkFeedInPlace())
                printTaskList.pop();
        } break;
        case CheckManInPlace: {
            if(checkManInPlace())
                printTaskList.pop();
        } break;
        case CheckPowerInPlace: {
            if(checkPowerInPlace())
                printTaskList.pop();
        } break;
        case SlicelayerScan: {
            slicelayerScan();
            printTaskList.pop();
        } break;
        case GenerateCurrentLayer: {
            generateCurrentLayer();
            printTaskList.pop();
        } break;
        case PrintFinish : {
            printFinish();
            printTaskList.pop();
        } break;
        default:break;
        }
    }


    qDebug()<<"doTask C:" << printTaskList.size_approx();

}

int TaskAgent::checkPrintReady()
{
#ifdef Eprint
    //test
    int   result = 7;

#else
    int result{0};
    if(firstPowderSpreaded){
        result+=1;
    }

    if(baseHeating){
        result+=2;
    }

    if(scrubbing){
        result+=4;
    }

#endif
    return result;
}

void TaskAgent::moveFeedServoAvoid()
{
    double feedPos = SYSTEMMONITOR.getFeedPos();
    feedTargetPos = feedPos + avoidanceDistance;
    SERVOAGENT.moveJog(FEED , avoidanceDistance);
    qDebug()<<"feedPos :" <<feedPos << "feedTargetPos :" <<feedTargetPos;
}

void TaskAgent::moveManServoAvoid()
{
    double manPos = SYSTEMMONITOR.getManPos();
    manTargetPos = manPos + avoidanceDistance;
    SERVOAGENT.moveJog(MANUFACTURE , avoidanceDistance);
    qDebug()<<"manPos :" <<manPos << "manTargetPos :" <<manTargetPos;
}

void TaskAgent::moveFeedSpreading()
{
    double feedPos = SYSTEMMONITOR.getFeedPos();
    feedTargetPos = feedPos - avoidanceDistance -  layerHigh*adjPcMulValue;
    SERVOAGENT.moveJog(FEED , -avoidanceDistance - layerHigh*adjPcMulValue);

    qDebug()<<"spreading feedPos:" <<feedPos << "feedTargetPos :" << feedTargetPos << "distance :" << -avoidanceDistance - layerHigh*adjPcMulValue;
}

void TaskAgent::moveManSpreading()
{
    double manPos = SYSTEMMONITOR.getManPos();
    manTargetPos = manPos - avoidanceDistance + layerHigh;
    SERVOAGENT.moveJog(MANUFACTURE , -avoidanceDistance + layerHigh );

    qDebug()<<"manPos spreading :" <<manPos << "manTargetPos :" << manTargetPos << "distance :" << -avoidanceDistance + layerHigh;

}

void TaskAgent::scraperToZero()
{
    powerTargetPos = 0;
    SERVOAGENT.scraperToZore();

    qDebug()<<"scraperToZore : powerTargetPos" <<powerTargetPos ;
}

void TaskAgent::scraperCover()
{
    powerTargetPos = SERVOAGENT.getPowerPosMax();
    SERVOAGENT.scraperSpreading(adjScraperSpeedRatio);

    qDebug()<<"scraperCover : powerTargetPos" <<powerTargetPos ;
}

bool TaskAgent::checkFeedInPlace()
{
    qDebug()<<"checkFeedInPlace pos: " << SYSTEMMONITOR.getFeedPos()<<"feedTargetPos : " << feedTargetPos;

    if(abs(SYSTEMMONITOR.getFeedPos()-feedTargetPos)<0.1){
        return SYSTEMMONITOR.getFeedInPlace();
    }else
        return false;
}

bool TaskAgent::checkManInPlace()
{
    qDebug()<<"checkManInPlace pos: " << SYSTEMMONITOR.getManPos()<<"manTargetPos : " << manTargetPos;
    if(abs(SYSTEMMONITOR.getManPos()-manTargetPos)<0.1)
        return SYSTEMMONITOR.getManInPlace();
    else
        return false;
}

bool TaskAgent::checkPowerInPlace()
{
    qDebug()<<"checkPowerInPlace pos: " << SYSTEMMONITOR.getPowPos()<<"powerTargetPos : " << powerTargetPos;
    if(abs(SYSTEMMONITOR.getPowPos()-powerTargetPos)<5)
        return SYSTEMMONITOR.getPowInPlace();
    else
        return false;
}

void TaskAgent::slicelayerScan()
{
    RTC5AGENT.scanSlicelayer();
    qDebug()<<"scan Finish ";
}

void TaskAgent::generateCurrentLayer()
{
    int curruntLayerIndex = CLIREAD.getCurruntLayerIndex();
    emit updataCliShow(curruntLayerIndex+1);
    qDebug()<<"curruntLayerIndex :" << curruntLayerIndex;
}

void TaskAgent::printFinish()
{
    threadPause.store(true);
    isTaskWork.store(false);
    firstPowderSpreaded = false;
    baseHeating = false;
    scrubbing = false;
    emit printFinished();
}

void TaskAgent::startPrintTask()
{
    if(isTaskWork.load()){
        threadPause.store(false);
    }else{
        RTC5AGENT.ApplyLaserConfig();
 #ifdef Eprint
        RTC5AGENT.ctlSetLaserPinOut(1);
        RTC5AGENT.ctlEnableLaser(false);
#else
        RTC5AGENT.ctlEnableLaser(true);
#endif
        int totalLayers  = CLIREAD.getCliHeasderData()->layers-2;
        printTaskList.enqueue(SlicelayerScan);

        /* 成型下降1mm             *
         * 供粉下降1mm             *
         * 成型下降到位             *
         * 供粉下降到位             *
         * 刮刀移动到0             *
         * 解析下一层             *
         * 刮刀移动到位             *
         * 成型上升1mm-层厚             *
         * 供粉上升1mm+层厚*供粉系数             *
         * 成型移动到位             *
         * 供粉移动到位             *
         * 刮刀移动到最大             *
         * 刮刀移动到位             *
         * 当前层扫描             *
         */
        qDebug()<<"totalLayers:"<<totalLayers;
        for(int i = 0 ; i < totalLayers ; i++){
            printTaskList.enqueue(MoveManServoAvoid);
            printTaskList.enqueue(MoveFeedServoAvoid);
            printTaskList.enqueue(CheckManInPlace);
            printTaskList.enqueue(CheckFeedInPlace);
            printTaskList.enqueue(ScraperToZero);
            printTaskList.enqueue(GenerateCurrentLayer);
            printTaskList.enqueue(CheckPowerInPlace);
            printTaskList.enqueue(MoveFeedSpreading);
            printTaskList.enqueue(MoveManSpreading);
            printTaskList.enqueue(CheckFeedInPlace);
            printTaskList.enqueue(CheckManInPlace);
            printTaskList.enqueue(ScraperCover);
            printTaskList.enqueue(CheckPowerInPlace);
            printTaskList.enqueue(SlicelayerScan);
        }
        printTaskList.enqueue(PrintFinish);
        qDebug()<<"printTaskList.size_approx():"<<printTaskList.size_approx();
        taskAgentThread ->start();
        threadPause.store(false);
        isTaskWork.store(true);
    }
}

bool TaskAgent::pausePrintTask()
{
    if(isTaskWork.load()){
        threadPause.store(true);
        return true;
    }else{
        return false;
    }
}

bool TaskAgent::stopPrintTask()
{
    if(isTaskWork.load()){
        threadPause.store(true);
        isTaskWork.store(false);
        firstPowderSpreaded = false;
        baseHeating = false;
        scrubbing = false;
        RTC5AGENT.MarkStop();
        SERVOAGENT.moveStop(FEED);
        SERVOAGENT.moveStop(MANUFACTURE);
        SERVOAGENT.moveStop(POWDER);
        return true;
    }else{
        return false;
    }




    //TODO
    //循环风机停止
    //清理任务列表
    //退出所有







}

}
